#ifndef MOTOR_CONTROLLER_HPP
#define MOTOR_CONTROLLER_HPP

#include "PIDController.hpp"
#include "PiecewiseLinearInterpolation.hpp"
#include "Configuration.hpp"
#include "adc.hpp"
#include "Configuration.hpp"

extern SystemConfiguration system_configuration;

class MotorController{
public:
    MotorController(ADCUtil *adc_util);
    ~MotorController();
    bool init(void);
    void loop(void);
    void setPosition(double pos);
    double getPosition(void);
    double getPositionSetPoint(void);
    void debugPrint(void);
    void updatePli(void);
    double getSetPosition(void);
// private:
    void setForce(double f);
    double getForce(void);
private:
    ADCUtil *adc_util = nullptr;
    PIDController *pid_controller;
    PiecewiseLinearInterpolation *pli;
    double position_setpoint = 0.0;
    double motor_motion_margin = 0.0;
    double motor_force = 0.0;
};

#endif
